﻿using Microsoft.Kinect;
using System;
using System.Collections.Generic;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Media;
using Microsoft.Kinect.Toolkit.FaceTracking;
using Point = System.Windows.Point;
using System.Diagnostics;

namespace PupillaryDistance.DesktopUI
{
    public partial class FaceTrackingViewer : UserControl, IDisposable
    {
        private const int RIGHT_TOP_OUTER_PUPIL = 70;
        private const int RIGHT_BOTTOM_OUTER_PUPIL = 71;
        private const int RIGHT_TOP_INNER_PUPIL = 74;
        private const int RIGHT_BOTTOM_INNER_PUPIL = 75;
        private const int LEFT_TOP_OUTER_PUPIL = 68;
        private const int LEFT_BOTTOM_OUTER_PUPIL = 69;
        private const int LEFT_TOP_INNER_PUPIL = 72;
        private const int LEFT_BOTTOM_INNER_PUPIL = 73;
        private const int RIGHT_TOP_CENTER_PUPIL = 55;
        private const int RIGHT_BOTTOM_CENTER_PUPIL = 56;
        private const int LEFT_TOP_CENTER_PUPIL = 22;
        private const int LEFT_BOTTOM_CENTER_PUPIL = 23;
        private const int RIGHT_OUTER = 54;
        private const int RIGHT_INNER = 57;
        private const int LEFT_OUTER = 21;
        private const int LEFT_INNER = 24;

        public bool IsBeingTracked { get; set; }
        public bool MethodOneEnabled { get; set; }
        public bool MethodTwoEnabled { get; set; }
        public bool MethodThreeEnabled { get; set; }
        public bool MethodFourEnabled { get; set; }
        public bool MethodFiveEnabled { get; set; }
        public bool EyeTracker { get; set; }

        public static readonly DependencyProperty KinectProperty = DependencyProperty.Register(
            "Kinect",
            typeof(KinectSensor),
            typeof(FaceTrackingViewer),
            new PropertyMetadata(
                null, (o, args) => ((FaceTrackingViewer)o).OnSensorChanged((KinectSensor)args.OldValue, (KinectSensor)args.NewValue)));

        private const uint MaxMissedFrames = 100;
        private readonly Dictionary<int, SkeletonFaceTracker> trackedSkeletons = new Dictionary<int, SkeletonFaceTracker>();
        private byte[] colorImage;
        private ColorImageFormat colorImageFormat = ColorImageFormat.Undefined;
        private short[] depthImage;
        private DepthImageFormat depthImageFormat = DepthImageFormat.Undefined;
        private bool disposed;
        private Skeleton[] skeletonData;

        public FaceTrackingViewer()
        {
            this.InitializeComponent();
        }

        ~FaceTrackingViewer()
        {
            this.Dispose(false);
        }

        public KinectSensor Kinect
        {
            get
            {
                return (KinectSensor)this.GetValue(KinectProperty);
            }
            set
            {
                this.SetValue(KinectProperty, value);
            }
        }

        public void Dispose()
        {
            this.Dispose(true);
            GC.SuppressFinalize(this);
        }

        protected virtual void Dispose(bool disposing)
        {
            if (!this.disposed)
            {
                this.ResetFaceTracking();

                this.disposed = true;
            }
        }

        protected override void OnRender(DrawingContext drawingContext)
        {
            base.OnRender(drawingContext);
            foreach (SkeletonFaceTracker faceInformation in this.trackedSkeletons.Values)
            {
                faceInformation.DrawFaceModel(drawingContext);
            }

           
        }

        private void OnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
        {
            ColorImageFrame colorImageFrame = null;
            DepthImageFrame depthImageFrame = null;
            SkeletonFrame skeletonFrame = null;

            try
            {
                colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame();
                depthImageFrame = allFramesReadyEventArgs.OpenDepthImageFrame();
                skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame();

                if (colorImageFrame == null || depthImageFrame == null || skeletonFrame == null)
                {
                    return;
                }

                // Check for image format changes.  The FaceTracker doesn't
                // deal with that so we need to reset.
                if (this.depthImageFormat != depthImageFrame.Format)
                {
                    this.ResetFaceTracking();
                    this.depthImage = null;
                    this.depthImageFormat = depthImageFrame.Format;
                }

                if (this.colorImageFormat != colorImageFrame.Format)
                {
                    this.ResetFaceTracking();
                    this.colorImage = null;
                    this.colorImageFormat = colorImageFrame.Format;
                }

                // Create any buffers to store copies of the data we work with
                if (this.depthImage == null)
                {
                    this.depthImage = new short[depthImageFrame.PixelDataLength];
                }

                if (this.colorImage == null)
                {
                    this.colorImage = new byte[colorImageFrame.PixelDataLength];
                }

                // Get the skeleton information
                if (this.skeletonData == null || this.skeletonData.Length != skeletonFrame.SkeletonArrayLength)
                {
                    this.skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
                }

                colorImageFrame.CopyPixelDataTo(this.colorImage);
                depthImageFrame.CopyPixelDataTo(this.depthImage);
                skeletonFrame.CopySkeletonDataTo(this.skeletonData);

                // Update the list of trackers and the trackers with the current frame information
                foreach (Skeleton skeleton in this.skeletonData)
                {
                    if (skeleton.TrackingState == SkeletonTrackingState.Tracked
                        || skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
                    {
                        // We want keep a record of any skeleton, tracked or untracked.
                        if (!this.trackedSkeletons.ContainsKey(skeleton.TrackingId))
                        {
                            this.trackedSkeletons.Add(skeleton.TrackingId, new SkeletonFaceTracker());
                        }

                        // Give each tracker the upated frame.
                        SkeletonFaceTracker skeletonFaceTracker;
                        if (this.trackedSkeletons.TryGetValue(skeleton.TrackingId, out skeletonFaceTracker))
                        {
                            skeletonFaceTracker.OnFrameReady(this.Kinect, colorImageFormat, colorImage, depthImageFormat, depthImage, skeleton, this);
                            skeletonFaceTracker.LastTrackedFrame = skeletonFrame.FrameNumber;
                        }
                    }
                }

                this.RemoveOldTrackers(skeletonFrame.FrameNumber);

                this.InvalidateVisual();
            }
            finally
            {
                if (colorImageFrame != null)
                {
                    colorImageFrame.Dispose();
                }

                if (depthImageFrame != null)
                {
                    depthImageFrame.Dispose();
                }

                if (skeletonFrame != null)
                {
                    skeletonFrame.Dispose();
                }
            }
        }

        private void OnSensorChanged(KinectSensor oldSensor, KinectSensor newSensor)
        {
            if (oldSensor != null)
            {
                oldSensor.AllFramesReady -= this.OnAllFramesReady;
                this.ResetFaceTracking();
            }

            if (newSensor != null)
            {
                newSensor.AllFramesReady += this.OnAllFramesReady;
            }
        }

        /// <summary>
        /// Clear out any trackers for skeletons we haven't heard from for a while
        /// </summary>
        private void RemoveOldTrackers(int currentFrameNumber)
        {
            var trackersToRemove = new List<int>();

            foreach (var tracker in this.trackedSkeletons)
            {
                uint missedFrames = (uint)currentFrameNumber - (uint)tracker.Value.LastTrackedFrame;
                if (missedFrames > MaxMissedFrames)
                {
                    // There have been too many frames since we last saw this skeleton
                    trackersToRemove.Add(tracker.Key);
                }
            }

            foreach (int trackingId in trackersToRemove)
            {
                this.RemoveTracker(trackingId);
            }
        }

        private void RemoveTracker(int trackingId)
        {
            this.trackedSkeletons[trackingId].Dispose();
            this.trackedSkeletons.Remove(trackingId);
        }

        private void ResetFaceTracking()
        {
            foreach (int trackingId in new List<int>(this.trackedSkeletons.Keys))
            {
                this.RemoveTracker(trackingId);
            }
        }

        private class SkeletonFaceTracker : IDisposable
        {
            private static FaceTriangle[] faceTriangles;
            private EnumIndexableCollection<FeaturePoint, PointF> facePoints;
            private EnumIndexableCollection<FeaturePoint, Vector3DF> face3dPoints;
            private FaceTracker faceTracker;
            private bool lastFaceTrackSucceeded;
            private SkeletonTrackingState skeletonTrackingState;
            public int LastTrackedFrame { get; set; }
            private FaceTrackingViewer _ftv;

            public void Dispose()
            {
                if (this.faceTracker != null)
                {
                    this.faceTracker.Dispose();
                    this.faceTracker = null;
                }
            }

            public void DrawFaceModel(DrawingContext drawingContext)
            {
      
                if (!this.lastFaceTrackSucceeded || this.skeletonTrackingState != SkeletonTrackingState.Tracked)
                {
                    return;
                }

                var faceModelPts = new List<Point>();
                var faceModel3dPts = new List<Vector3DF>();

                for (int i = 0; i < this.facePoints.Count; i++)
                {
                    faceModelPts.Add(new Point(this.facePoints[i].X, this.facePoints[i].Y));
                    faceModel3dPts.Add(new Vector3DF(this.face3dPoints[i].X, this.face3dPoints[i].Y, this.face3dPoints[i].Z));
                }

                #region Drawing Eyes
                if (_ftv.EyeTracker)
                {
                    // right eye
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[54 - 1].X, faceModelPts[54 - 1].Y),
                            new Point(faceModelPts[70 - 1].X, faceModelPts[70 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[70 - 1].X, faceModelPts[70 - 1].Y),
                            new Point(faceModelPts[74 - 1].X, faceModelPts[74 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[74 - 1].X, faceModelPts[74 - 1].Y),
                            new Point(faceModelPts[57 - 1].X, faceModelPts[57 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[57 - 1].X, faceModelPts[57 - 1].Y),
                            new Point(faceModelPts[75 - 1].X, faceModelPts[75 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[75 - 1].X, faceModelPts[75 - 1].Y),
                            new Point(faceModelPts[71 - 1].X, faceModelPts[71 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[71 - 1].X, faceModelPts[71 - 1].Y),
                            new Point(faceModelPts[54 - 1].X, faceModelPts[54 - 1].Y));
                    // left eye
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[24 - 1].X, faceModelPts[24 - 1].Y),
                            new Point(faceModelPts[72 - 1].X, faceModelPts[72 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[72 - 1].X, faceModelPts[72 - 1].Y),
                            new Point(faceModelPts[68 - 1].X, faceModelPts[68 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[68 - 1].X, faceModelPts[68 - 1].Y),
                            new Point(faceModelPts[21 - 1].X, faceModelPts[21 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[21 - 1].X, faceModelPts[21 - 1].Y),
                            new Point(faceModelPts[69 - 1].X, faceModelPts[69 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[69 - 1].X, faceModelPts[69 - 1].Y),
                            new Point(faceModelPts[73 - 1].X, faceModelPts[73 - 1].Y));
                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0),
                            new Point(faceModelPts[73 - 1].X, faceModelPts[73 - 1].Y),
                            new Point(faceModelPts[24 - 1].X, faceModelPts[24 - 1].Y));
                }
                #endregion

                #region Method One

                if (_ftv.MethodOneEnabled)
                {
                    drawingContext.DrawLine(new Pen(Brushes.LightGreen, 1.0),
                        new Point(faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].X, faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].Y),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y));

                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkGreen, 1),
                        new Point(faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].X, faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].Y), 5, 5);
                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkGreen, 1),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y), 5, 5);

                    double methodOnePart1 = Math.Pow((faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].X - faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].X), 2);
                    double methodOnePart2 = Math.Pow((faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].Y - faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].Y), 2);
                    double methodOnePart3 = Math.Pow((faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].Z - faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].Z), 2);
                    double underRadical = methodOnePart1 + methodOnePart2 + methodOnePart3;
                    double methodOneLength = Math.Sqrt(underRadical) * 1000;

                    drawingContext.DrawText(new FormattedText("M1: " + Math.Round(methodOneLength, 3).ToString() + " mm",
                        System.Globalization.CultureInfo.CurrentCulture, FlowDirection.LeftToRight,
                        new Typeface("Consolas"), 30, Brushes.DarkGreen),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + 50, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y - 90));
                }

                #endregion

                #region Method Two

                if (_ftv.MethodTwoEnabled)
                {
                    // 2d Drawing

                    double avgLeftTop2dX = (faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].X + faceModelPts[LEFT_TOP_INNER_PUPIL - 1].X) / 2;
                    double avgLeftBottom2dX = (faceModelPts[LEFT_BOTTOM_OUTER_PUPIL - 1].X + faceModelPts[LEFT_BOTTOM_INNER_PUPIL - 1].X) / 2;
                    double avgLeft2dX = (avgLeftTop2dX + avgLeftBottom2dX) / 2;

                    double avgRightTop2dX = (faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + faceModelPts[RIGHT_TOP_INNER_PUPIL - 1].X) / 2;
                    double avgRightBottom2dX = (faceModelPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].X + faceModelPts[RIGHT_BOTTOM_INNER_PUPIL - 1].X) / 2;
                    double avgRight2dX = (avgRightTop2dX + avgRightBottom2dX) / 2;

                    double avgLeftTop2dY = (faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].Y + faceModelPts[LEFT_TOP_INNER_PUPIL - 1].Y) / 2;
                    double avgLeftBottom2dY = (faceModelPts[LEFT_BOTTOM_OUTER_PUPIL - 1].Y + faceModelPts[LEFT_BOTTOM_INNER_PUPIL - 1].Y) / 2;
                    double avgLeft2dY = (avgLeftTop2dY + avgLeftBottom2dY) / 2;

                    double avgRightTop2dY = (faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y + faceModelPts[RIGHT_TOP_INNER_PUPIL - 1].Y) / 2;
                    double avgRightBottom2dY = (faceModelPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].Y + faceModelPts[RIGHT_BOTTOM_INNER_PUPIL - 1].Y) / 2;
                    double avgRight2dY = (avgRightTop2dY + avgRightBottom2dY) / 2;

                    drawingContext.DrawLine(new Pen(Brushes.LightBlue, 1.0), new Point(avgLeft2dX, avgLeft2dY),
                       new Point(avgRight2dX, avgRight2dY));

                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkBlue, 1), new Point(avgLeft2dX, avgLeft2dY), 5, 5);
                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkBlue, 1), new Point(avgRight2dX, avgRight2dY), 5, 5);

                    // 3d

                    double avgLeftTopX = (faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].X + faceModel3dPts[LEFT_TOP_INNER_PUPIL - 1].X) / 2;
                    double avgLeftBottomX = (faceModel3dPts[LEFT_BOTTOM_OUTER_PUPIL - 1].X + faceModel3dPts[LEFT_BOTTOM_INNER_PUPIL - 1].X) / 2;
                    double avgLeftX = (avgLeftTopX + avgLeftBottomX) / 2;

                    double avgRightTopX = (faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].X + faceModel3dPts[RIGHT_TOP_INNER_PUPIL - 1].X) / 2;
                    double avgRightBottomX = (faceModel3dPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].X + faceModel3dPts[RIGHT_BOTTOM_INNER_PUPIL - 1].X) / 2;
                    double avgRightX = (avgRightTopX + avgRightBottomX) / 2;

                    double avgLeftTopY = (faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].Y + faceModel3dPts[LEFT_TOP_INNER_PUPIL - 1].Y) / 2;
                    double avgLeftBottomY = (faceModel3dPts[LEFT_BOTTOM_OUTER_PUPIL - 1].Y + faceModel3dPts[LEFT_BOTTOM_INNER_PUPIL - 1].Y) / 2;
                    double avgLeftY = (avgLeftTopY + avgLeftBottomY) / 2;

                    double avgRightTopY = (faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].Y + faceModel3dPts[RIGHT_TOP_INNER_PUPIL - 1].Y) / 2;
                    double avgRightBottomY = (faceModel3dPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].Y + faceModel3dPts[RIGHT_BOTTOM_INNER_PUPIL - 1].Y) / 2;
                    double avgRightY = (avgRightTopY + avgRightBottomY) / 2;

                    double avgLeftTopZ = (faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].Z + faceModel3dPts[LEFT_TOP_INNER_PUPIL - 1].Z) / 2; ;
                    double avgLeftBottomZ = (faceModel3dPts[LEFT_BOTTOM_OUTER_PUPIL - 1].Z + faceModel3dPts[LEFT_BOTTOM_INNER_PUPIL - 1].Z) / 2;
                    double avgLeftZ = (avgLeftTopZ + avgLeftBottomZ) / 2;

                    double avgRightTopZ = (faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].Z + faceModel3dPts[RIGHT_TOP_INNER_PUPIL - 1].Z) / 2;
                    double avgRightBottomZ = (faceModel3dPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].Z + faceModel3dPts[RIGHT_BOTTOM_INNER_PUPIL - 1].Z) / 2;
                    double avgRightZ = (avgRightTopZ + avgRightBottomZ) / 2;

                    double methodTwoPart1 = Math.Pow((avgLeftX - avgRightX), 2);
                    double methodTwoPart2 = Math.Pow((avgLeftY - avgRightY), 2);
                    double methodTwoPart3 = Math.Pow((avgLeftZ - avgRightZ), 2);

                    double methodTwoUR = methodTwoPart1 + methodTwoPart2 + methodTwoPart3;
                    double methodTwoLength = Math.Sqrt(methodTwoUR) * 1000;

                    drawingContext.DrawText(new FormattedText("M2: " + Math.Round(methodTwoLength, 3).ToString() + " mm",
                        System.Globalization.CultureInfo.CurrentCulture, FlowDirection.LeftToRight,
                        new Typeface("Consolas"), 30, Brushes.DarkBlue),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + 50, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y - 60));
                }
                #endregion

                #region Method Three

                if (_ftv.MethodThreeEnabled)
                {
                    double leftAverage2dX = (faceModelPts[RIGHT_TOP_CENTER_PUPIL - 1].X + faceModelPts[RIGHT_BOTTOM_CENTER_PUPIL - 1].X) / 2;
                    double rightAverage2dX = (faceModelPts[LEFT_TOP_CENTER_PUPIL - 1].X + faceModelPts[LEFT_BOTTOM_CENTER_PUPIL - 1].X) / 2;
                    double leftAverage2dY = (faceModelPts[RIGHT_TOP_CENTER_PUPIL - 1].Y + faceModelPts[RIGHT_BOTTOM_CENTER_PUPIL - 1].Y) / 2;
                    double rightAverage2dY = (faceModelPts[LEFT_TOP_CENTER_PUPIL - 1].Y + faceModelPts[LEFT_BOTTOM_CENTER_PUPIL - 1].Y) / 2;

                    drawingContext.DrawLine(new Pen(Brushes.Pink, 1.0), new Point(leftAverage2dX, leftAverage2dY),
                       new Point(rightAverage2dX, rightAverage2dY));

                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkRed, 1), new Point(leftAverage2dX, leftAverage2dY), 5, 5);
                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkRed, 1), new Point(rightAverage2dX, rightAverage2dY), 5, 5);

                    double leftAverageX = (faceModel3dPts[RIGHT_TOP_CENTER_PUPIL - 1].X + faceModel3dPts[RIGHT_BOTTOM_CENTER_PUPIL - 1].X) / 2;
                    double rightAverageX = (faceModel3dPts[LEFT_TOP_CENTER_PUPIL - 1].X + faceModel3dPts[LEFT_BOTTOM_CENTER_PUPIL - 1].X) / 2;
                    double leftAverageY = (faceModel3dPts[RIGHT_TOP_CENTER_PUPIL - 1].Y + faceModel3dPts[RIGHT_BOTTOM_CENTER_PUPIL - 1].Y) / 2;
                    double rightAverageY = (faceModel3dPts[LEFT_TOP_CENTER_PUPIL - 1].Y + faceModel3dPts[LEFT_BOTTOM_CENTER_PUPIL - 1].Y) / 2;
                    double leftAverageZ = (faceModel3dPts[RIGHT_TOP_CENTER_PUPIL - 1].Z + faceModel3dPts[RIGHT_BOTTOM_CENTER_PUPIL - 1].Z) / 2;
                    double rightAverageZ = (faceModel3dPts[LEFT_TOP_CENTER_PUPIL - 1].Z + faceModel3dPts[LEFT_BOTTOM_CENTER_PUPIL - 1].Z) / 2;

                    double methodThreePart1 = Math.Pow((leftAverageX - rightAverageX), 2);
                    double methodThreePart2 = Math.Pow((leftAverageY - rightAverageY), 2);
                    double methodThreePart3 = Math.Pow((leftAverageZ - rightAverageZ), 2);

                    double underRadical = methodThreePart1 + methodThreePart2 + methodThreePart3;
                    double methodThreeLength = Math.Sqrt(underRadical) * 1000;

                    drawingContext.DrawText(new FormattedText("M3: " + Math.Round(methodThreeLength, 3).ToString() + " mm",
                        System.Globalization.CultureInfo.CurrentCulture, FlowDirection.LeftToRight,
                        new Typeface("Consolas"), 30, Brushes.DarkRed),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + 50, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y - 30));
                }
                #endregion

                #region Method Four

                if (_ftv.MethodFourEnabled)
                {
                    double leftAverage2dX = (faceModelPts[LEFT_OUTER - 1].X + faceModelPts[LEFT_INNER - 1].X) / 2;
                    double rightAverage2dX = (faceModelPts[RIGHT_OUTER - 1].X + faceModelPts[RIGHT_INNER - 1].X) / 2;
                    double leftAverage2dY = (faceModelPts[LEFT_OUTER - 1].Y + faceModelPts[LEFT_INNER - 1].Y) / 2;
                    double rightAverage2dY = (faceModelPts[RIGHT_OUTER - 1].Y + faceModelPts[RIGHT_INNER - 1].Y) / 2;

                    drawingContext.DrawLine(new Pen(Brushes.LightYellow, 1.0), new Point(leftAverage2dX, leftAverage2dY),
                       new Point(rightAverage2dX, rightAverage2dY));

                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkGoldenrod, 1), new Point(leftAverage2dX, leftAverage2dY), 5, 5);
                    drawingContext.DrawEllipse(null, new Pen(Brushes.DarkGoldenrod, 1), new Point(rightAverage2dX, rightAverage2dY), 5, 5);

                    double leftAverageX = (faceModel3dPts[LEFT_OUTER - 1].X + faceModel3dPts[LEFT_INNER - 1].X) / 2;
                    double rightAverageX = (faceModel3dPts[RIGHT_OUTER - 1].X + faceModel3dPts[RIGHT_INNER - 1].X) / 2;
                    double leftAverageY = (faceModel3dPts[LEFT_OUTER - 1].Y + faceModel3dPts[LEFT_INNER - 1].Y) / 2;
                    double rightAverageY = (faceModel3dPts[RIGHT_OUTER - 1].Y + faceModel3dPts[RIGHT_INNER - 1].Y) / 2;
                    double leftAverageZ = (faceModel3dPts[LEFT_OUTER - 1].Z + faceModel3dPts[LEFT_INNER - 1].Z) / 2;
                    double rightAverageZ = (faceModel3dPts[RIGHT_OUTER - 1].Z + faceModel3dPts[RIGHT_INNER - 1].Z) / 2;

                    double methodThreePart1 = Math.Pow((leftAverageX - rightAverageX), 2);
                    double methodThreePart2 = Math.Pow((leftAverageY - rightAverageY), 2);
                    double methodThreePart3 = Math.Pow((leftAverageZ - rightAverageZ), 2);

                    double underRadical = methodThreePart1 + methodThreePart2 + methodThreePart3;
                    double methodThreeLength = Math.Sqrt(underRadical) * 1000;

                    drawingContext.DrawText(new FormattedText("M4: " + Math.Round(methodThreeLength, 3).ToString() + " mm",
                        System.Globalization.CultureInfo.CurrentCulture, FlowDirection.LeftToRight,
                        new Typeface("Consolas"), 30, Brushes.DarkGoldenrod),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + 50, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y));
                }

                #endregion

                #region Method Five

                if (_ftv.MethodFiveEnabled)
                {
                    double leftAverage2dX = (faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + faceModelPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].X) / 2;
                    double rightAverage2dX = (faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].X + faceModelPts[LEFT_BOTTOM_OUTER_PUPIL - 1].X) / 2;
                    double leftAverage2dY = (faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y + faceModelPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].Y) / 2;
                    double rightAverage2dY = (faceModelPts[LEFT_TOP_OUTER_PUPIL - 1].Y + faceModelPts[LEFT_BOTTOM_OUTER_PUPIL - 1].Y) / 2;

                    drawingContext.DrawLine(new Pen(Brushes.White, 1.0), new Point(leftAverage2dX, leftAverage2dY),
                       new Point(rightAverage2dX, rightAverage2dY));

                    drawingContext.DrawEllipse(null, new Pen(Brushes.Gray, 1), new Point(leftAverage2dX, leftAverage2dY), 5, 5);
                    drawingContext.DrawEllipse(null, new Pen(Brushes.Gray, 1), new Point(rightAverage2dX, rightAverage2dY), 5, 5);

                    double leftAverageX = (faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].X + faceModel3dPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].X) / 2;
                    double rightAverageX = (faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].X + faceModel3dPts[LEFT_BOTTOM_OUTER_PUPIL - 1].X) / 2;
                    double leftAverageY = (faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].Y + faceModel3dPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].Y) / 2;
                    double rightAverageY = (faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].Y + faceModel3dPts[LEFT_BOTTOM_OUTER_PUPIL - 1].Y) / 2;
                    double leftAverageZ = (faceModel3dPts[RIGHT_TOP_OUTER_PUPIL - 1].Z + faceModel3dPts[RIGHT_BOTTOM_OUTER_PUPIL - 1].Z) / 2;
                    double rightAverageZ = (faceModel3dPts[LEFT_TOP_OUTER_PUPIL - 1].Z + faceModel3dPts[LEFT_BOTTOM_OUTER_PUPIL - 1].Z) / 2;

                    double methodThreePart1 = Math.Pow((leftAverageX - rightAverageX), 2);
                    double methodThreePart2 = Math.Pow((leftAverageY - rightAverageY), 2);
                    double methodThreePart3 = Math.Pow((leftAverageZ - rightAverageZ), 2);

                    double underRadical = methodThreePart1 + methodThreePart2 + methodThreePart3;
                    double methodThreeLength = Math.Sqrt(underRadical) * 1000;

                    drawingContext.DrawText(new FormattedText("M5: " + Math.Round(methodThreeLength, 3).ToString() + " mm",
                        System.Globalization.CultureInfo.CurrentCulture, FlowDirection.LeftToRight,
                        new Typeface("Consolas"), 30, Brushes.Gray),
                        new Point(faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].X + 50, faceModelPts[RIGHT_TOP_OUTER_PUPIL - 1].Y + 30));
                }
                #endregion
            }

            /// <summary>
            /// Updates the face tracking information for this skeleton
            /// </summary>
            internal void OnFrameReady(KinectSensor kinectSensor, ColorImageFormat colorImageFormat, byte[] colorImage,
                DepthImageFormat depthImageFormat, short[] depthImage, Skeleton skeletonOfInterest, FaceTrackingViewer ftv)
            {
                _ftv = ftv;
                this.skeletonTrackingState = skeletonOfInterest.TrackingState;

                if (this.skeletonTrackingState != SkeletonTrackingState.Tracked)
                {
                    _ftv.IsBeingTracked = false;
                    // nothing to do with an untracked skeleton.
                    return;
                }
                _ftv.IsBeingTracked = true;

                if (this.faceTracker == null)
                {
                    try
                    {
                        this.faceTracker = new FaceTracker(kinectSensor);

                    }
                    catch (InvalidOperationException)
                    {
                        // During some shutdown scenarios the FaceTracker
                        // is unable to be instantiated.  Catch that exception
                        // and don't track a face.
                        Debug.WriteLine("AllFramesReady - creating a new FaceTracker threw an InvalidOperationException");
                        this.faceTracker = null;
                    }
                }

                if (this.faceTracker != null)
                {
                    FaceTrackFrame frame = this.faceTracker.Track(
                        colorImageFormat, colorImage, depthImageFormat, depthImage, skeletonOfInterest);

                    this.lastFaceTrackSucceeded = frame.TrackSuccessful;
                    if (this.lastFaceTrackSucceeded)
                    {
                        if (faceTriangles == null)
                        {
                            // only need to get this once.  It doesn't change.
                            faceTriangles = frame.GetTriangles();
                        }

                        this.facePoints = frame.GetProjected3DShape();
                        this.face3dPoints = frame.Get3DShape();
                    }
                }
            }
        }
    }
}
